using System;
using System.Collections.Generic;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics.CollisionDetectors
{
	/// <summary>
	/// Detects collisions between a <see cref="CollisionSphere"/> and a <see cref="CollisionPlane"/>.
	/// </summary>
	public class SpherePlaneCollisionDetector : CollisionDetector
	{
		#region CollisionDetector Members

		public override void Detect(CollisionShape shape0, CollisionShape shape1, CollisionContext context)
		{
			// Typecast the shapes to their correct subclasses.
			CollisionSphere sphere = (CollisionSphere)shape0;
			CollisionPlane plane = (CollisionPlane)shape1;

			// Calculate the distance between the sphere's center and the plane.
#if INLINE
			float distance = (plane.Normal.X * sphere.Position.X + plane.Normal.Y * sphere.Position.Y + plane.Normal.Z * sphere.Position.Z) - plane.Distance;
#elif REFERENCE
			float distance = Vector3Extension.Dot(ref plane.Normal, ref sphere.Position) + plane.Distance;
#else
			float distance = Vector3.Dot(plane.Normal, sphere.Position) + plane.Distance;
#endif
			// Determine if the sphere is close enough to the plane for a collision.
			if (distance > sphere.Radius + context.Tolerance)
			{
				return;
			}

			// Calculate the depth of the penetration due to the collision.
			float collisionDepth = sphere.Radius - distance;

			// Calculate the position of the collision.
#if INLINE
			float x = sphere.Position.X - plane.Normal.X * sphere.Radius;
			float y = sphere.Position.Y - plane.Normal.Y * sphere.Radius;
			float z = sphere.Position.Z - plane.Normal.Z * sphere.Radius;
#elif REFERENCE
			Vector3 collisionPosition;
			Vector3.Multiply(ref plane.Normal, sphere.Radius, out collisionPosition);
			Vector3.Subtract(ref sphere.Position, ref collisionPosition, out collisionPosition);
#else
			Vector3 collisionPosition = sphere.Position - plane.Normal * sphere.Radius;
#endif
			// Create the collision point.
			CollisionPoint point = CollisionPoint.Retrieve();
#if INLINE
			point.RelativePosition0.X = x - shape0.CollisionGroup.Position.X;
			point.RelativePosition0.Y = y - shape0.CollisionGroup.Position.Y;
			point.RelativePosition0.Z = z - shape0.CollisionGroup.Position.Z;

			point.RelativePosition1.X = x - shape1.CollisionGroup.Position.X;
			point.RelativePosition1.Y = y - shape1.CollisionGroup.Position.Y;
			point.RelativePosition1.Z = z - shape1.CollisionGroup.Position.Z;
#elif REFERENCE
			Vector3.Subtract(ref collisionPosition, ref shape0.CollisionGroup.Position, out point.RelativePosition0);
			Vector3.Subtract(ref collisionPosition, ref shape1.CollisionGroup.Position, out point.RelativePosition1);
#else
			point.RelativePosition0 = collisionPosition - shape0.CollisionGroup.Position;
			point.RelativePosition1 = collisionPosition - shape1.CollisionGroup.Position;
#endif
			point.Depth = collisionDepth;

			// Add the collision point to the collection.
			CollisionPointCollection collisionPoints = CollisionPointCollection.Retrieve();
			collisionPoints.Add(point);

			// Notify subscribers of the collision.
			context.OnCollided(shape0, shape1, ref plane.Normal, collisionPoints);

			// Recycle the collision point collection.
			CollisionPointCollection.Recycle(collisionPoints);
		}

		#endregion
	}
}
